#include <rclcpp/rclcpp.hpp>  
#include <cv_bridge/cv_bridge.h>  
#include <opencv2/opencv.hpp>  
#include "recognition_interface/srv/get_ped_pos.hpp"
#include "recognition_interface/msg/rectangle.hpp"

class PersonDetectionClient : public rclcpp::Node  
{  
public:  
    PersonDetectionClient() : Node("person_detection_client")  
    {  
        client_ = create_client<recognition_interface::srv::GetPedPos>("detect_person");  
        
        timer_ = create_wall_timer(  
            std::chrono::milliseconds(100),  
            std::bind(&PersonDetectionClient::timer_callback, this));  
            
        cap_.open(0);  
        if (!cap_.isOpened()) {  
            RCLCPP_ERROR(get_logger(), "Could not open camera");  
            return;  
        }  
        
        RCLCPP_INFO(get_logger(), "Person detection client is ready.");  
    }  

private:  
    void timer_callback()  
{  
    cv::Mat frame;  
    cap_ >> frame;  
    if (frame.empty()) {  
        RCLCPP_ERROR(get_logger(), "Failed to capture frame");  
        return;  
    }  

    auto request = std::make_shared<recognition_interface::srv::GetPedPos::Request>();  
    
    sensor_msgs::msg::Image ros_image;  
    cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toImageMsg(ros_image);  
    request->imgmsg = ros_image;  

    using ServiceResponseFuture =   
        rclcpp::Client<recognition_interface::srv::GetPedPos>::SharedFuture;  

    auto response_received_callback = [this, frame](ServiceResponseFuture future) {  
        auto result = future.get();  
        if (result->is_detected)  
        {  
            // 在图像上绘制检测结果  
            cv::Mat display_frame = frame.clone();
            for(size_t i = 0; i < result->rectangles.size(); ++i){
                cv::circle(display_frame,   
                        cv::Point(result->rectangles[i].side[0] + result->rectangles[i].side[2]/2, 
                                  result->rectangles[i].side[1] + result->rectangles[i].side[3]/2),  
                        5, cv::Scalar(0, 0, 255), -1);  
                cv::rectangle(display_frame,  
                            cv::Point(result->rectangles[i].side[0],  
                                    result->rectangles[i].side[1]),  
                            cv::Point(result->rectangles[i].side[0] + result->rectangles[i].side[2],  
                                    result->rectangles[i].side[1] + result->rectangles[i].side[3]),  
                            cv::Scalar(0, 255, 0), 2);
            }
            RCLCPP_INFO(get_logger(), "Detected %ld persons", result->rectangles.size());  
            
            cv::imshow("Person Detection", display_frame);  
        } else {  
            cv::imshow("Person Detection", frame);  
        }  
        cv::waitKey(40);  
    };  

    auto future_result = client_->async_send_request(request, response_received_callback);  
}

    rclcpp::Client<recognition_interface::srv::GetPedPos>::SharedPtr client_;  
    rclcpp::TimerBase::SharedPtr timer_;  
    cv::VideoCapture cap_;  
};  

int main(int argc, char* argv[])  
{  
    rclcpp::init(argc, argv);  
    auto node = std::make_shared<PersonDetectionClient>();  
    rclcpp::spin(node);  
    rclcpp::shutdown();  
    return 0;  
}
